/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
  Modification: Only some functions are referenced
**/
#include "fem_pos_dev_smo.h"
#include "fem_pos_dev_osqp_int.h"

FemPosDevSmo::FemPosDevSmo()
{
}

bool FemPosDevSmo::Solve(const std::vector<std::pair<double, double>>& raw_point2d,
                                    const std::vector<double>& bounds, std::vector<double>* opt_x,
                                    std::vector<double>* opt_y)
{
  return QpWithOsqp(raw_point2d, bounds, opt_x, opt_y);
}

bool FemPosDevSmo::QpWithOsqp(const std::vector<std::pair<double, double>>& raw_point2d,
                                         const std::vector<double>& bounds, std::vector<double>* opt_x,
                                         std::vector<double>* opt_y)
{
  if (opt_x == nullptr || opt_y == nullptr)
  {
    std::cout << "opt_x or opt_y is nullptr";
    return false;
  }

  FemPosDevOsqpInt solver;

  // time_limit set to be 0.0 meaning no time limit
  double time_limit = 0.0;
  bool verbose = false;
  bool scaled_termination = true;
  bool warm_start = true;

  solver.set_weight_fem_pos_deviation(glo_smo_par.weight_fem_pos_deviation);
  solver.set_weight_path_length(glo_smo_par.weight_path_length);
  solver.set_weight_ref_deviation(glo_smo_par.weight_ref_deviation);

  solver.set_max_iter(glo_smo_par.max_iter);
  solver.set_time_limit(time_limit);
  solver.set_verbose(verbose);
  solver.set_scaled_termination(scaled_termination);
  solver.set_warm_start(warm_start);

  solver.set_ref_points(raw_point2d);
  solver.set_bounds_around_refs(bounds);

  if (!solver.Solve())
  {
    return false;
  }

  *opt_x = solver.opt_x();
  *opt_y = solver.opt_y();
  return true;
}
